﻿using System;
using System.Threading;
using HardwareInterface;
using HardwareInterface.DistanceMeter;
using HardwareInterface.Servo;

namespace EMGunSharp
{
    class Program
    {
        static L1 meter;
        static CircleTargetTracker tracker;
        static FSServoChain servo;
        static LinearityMapper lmx;
        static LinearityMapper lmy;
        static Launcher launcher;
        public const int YMAX = 135;
        public const int XMAX = 80;
        public const int YMIN = -135;
        public const int XMIN = -60;
        static double motorx, motory;

        static void Main(string[] args)
        {
            launcher = new Launcher();
            Console.WriteLine("---------------------------------------------");
            Console.WriteLine("EMGunSharp 基于OpenCv4和.Net5的电磁炮控制程序");
            Console.WriteLine("By Developer_ken");
            Console.WriteLine("---------------------------------------------");
            ModeSelector.INIT();
            Thread.Sleep(500);
            if (ModeSelector.ManualMode()) MODE_MANUAL();
            else MODE_AUTO();
        }

        private static void MODE_MANUAL()
        {
            Console.WriteLine(">>> M <<<");
            servo = new FSServoChain(Environment.GetEnvironmentVariable("COM_SERVO"));
            servo.Init();
            servo.SetAngle(0, 0);
            servo.SetAngle(1, 0);
            Console.WriteLine("Servo OK");
            while (true)
            {
                launcher.Charge();
                var input = Console.ReadLine().ToLower();
                switch (input[0])
                {
                    case 'a':
                        servo.SetAngle(1, double.Parse(input[1..]));
                        Thread.Sleep(500);
                        launcher.Launch();
                        break;
                    case 'd':
                        var a = Physics.GetLaunchAngle(15, double.Parse(input[1..]));
                        Console.WriteLine("Angle=" + a);
                        servo.SetAngle(1, a);
                        Thread.Sleep(500);
                        launcher.Launch();
                        break;
                }
            }
        }

        private static void MODE_AUTO()
        {
            Console.WriteLine(">>> A <<<");
            Console.Write("Meter...");
            meter = new L1(Environment.GetEnvironmentVariable("COM_METER"));
            meter.Init();
            Console.WriteLine("OK");
            Console.Write("Visual...");
            lmx = new LinearityMapper();
            lmy = new LinearityMapper();
            lmx.AddPoint(-320, -35);
            lmx.AddPoint(-50, -5);
            lmx.AddPoint(-15, -2);
            lmx.AddPoint(0, 0);
            lmx.AddPoint(15, 2);
            lmx.AddPoint(50, 5);
            lmx.AddPoint(320, 35);

            lmy.AddPoint(-240, 15);
            lmy.AddPoint(0, 0);
            lmy.AddPoint(240, -15);
            tracker = new CircleTargetTracker(0, true);
            servo = new FSServoChain(Environment.GetEnvironmentVariable("COM_SERVO"));
            servo.Init();
            synced = false;
            servo.SetAngle(0, 0);
            servo.SetAngle(1, 0);
            tracker.TargetFoundEvent += Tracker_TargetFoundEvent;
            tracker.TargetGoneEvent += Tracker_TargetGoneEvent;
            servo.SetAngle(0, -600);
            servo.SetAngle(1, 0);
            Thread.Sleep(500);
            servo.SetAngle(0, 600, 10000);
            Console.WriteLine("OK");
            Console.WriteLine("All done.");
            launcher.Charge();
            while (true) Thread.Sleep(int.MaxValue);
        }

        private static void Tracker_TargetGoneEvent()
        {
            synced = false;
            servo.SetAngle(1, 0);
            servo.SetAngle(0, -600);
            Thread.Sleep(500);
            servo.SetAngle(0, 600, 10000);
        }

        struct LastPos
        {
            public bool exists;
            public double lastangle;
            public double lastax;
        }

        static LastPos lastx, lasty;
        static bool synced = false;

        private static void Tracker_TargetFoundEvent(CircleTargetTracker.Target obj)
        {
            bool ready = false;
            if (!synced)
            {
                motorx = servo.GetCurrentAngle(0);
                motory = servo.GetCurrentAngle(1);
                synced = true;
            }
            if (obj.X > (obj.R / 3))
            {
                //double current = servo.GetCurrentAngle(0);
                //var angle = sl.ReadDy(obj.X, -obj.X);
                motorx += lmx.MapX2Y(obj.X);
                servo.SetAngle(0, (short)((motorx) * 10));
            }
            else if (obj.X < -(obj.R / 3))
            {
                //舵机左转
                //double current = servo.GetCurrentAngle(0);
                //var angle = sl.ReadDy(obj.X, -obj.X);
                motorx += lmx.MapX2Y(obj.X);
                servo.SetAngle(0, (short)((short)(motorx) * 10));
            }
            else
            {
                ready = true;
            }

            if (obj.Y > (obj.R / 3))
            {
                //舵机上转
                //double current = servo.GetCurrentAngle(1);
                //var angle = sl.ReadDy(obj.Y, -obj.Y);
                //var angle = lmy.MapX2Y(obj.Y);
                motory += lmy.MapX2Y(obj.Y);
                servo.SetAngle(1, (short)((short)(motory) * 10));
            }
            else if (obj.Y < -(obj.R / 3))
            {
                //舵机下转
                //double current = servo.GetCurrentAngle(1);
                //var angle = sl.ReadDy(obj.Y, -obj.Y);
                motory += lmy.MapX2Y(obj.Y);
                servo.SetAngle(1, (short)((short)(motory) * 10));
            }
            else
            {
                ready = ready && true;
            }
            if (ready)
            {
                Console.WriteLine("\nMeter...");
                var distance = meter.MeasureOnce();
                Console.WriteLine("D=" + distance.Distance);
                double angle = Physics.GetLaunchAngle(15, distance.Distance);
                servo.SetAngle(1, angle, 500);
                motory = angle;
                Console.WriteLine("A=" + angle);
                Thread.Sleep(500);
                launcher.Launch();
                Thread.Sleep(500);
                launcher.Charge();
                Tracker_TargetGoneEvent();
                Thread.Sleep(1000);
            }
            else
            {
                Thread.Sleep(250);
            }
        }
    }
}
